#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include "../drv/drv_i2c.h"
#include "l3g4200.h"

//I2C address is wither D0 or D2 depending on SA0 
#define L3G4200D_ADDRESS		0x68
#define SENSIVITY_L3G4200D_250DPS       0.00875F

//calibration data
S_Gyro gyro_calib;
int gyro_calibrate = 0;

static unsigned char L3G4200DSetDRDY( int fd );

void L3G4200DInit( int fd )
{
	printf("L3G4200 Init\n");

	drv_i2cSetDevice(fd, L3G4200D_ADDRESS);
	drv_i2cWriteByte(fd, L3G4200D_CTRL_REG1, 0x0F);
	drv_i2cWriteByte(fd, L3G4200D_CTRL_REG2, 0x00);
	drv_i2cWriteByte(fd, L3G4200D_CTRL_REG3, 0x08);
	drv_i2cWriteByte(fd, L3G4200D_CTRL_REG4, 0x00);
        drv_i2cWriteByte(fd, L3G4200D_CTRL_REG5, 0x30);

	gyro_calib.x = 0;
	gyro_calib.y = 0;
	gyro_calib.z = 0;

	gyro_calibrate = 0;
}	


void L3G4200DCalibrate( int fd )
{
	printf("L3G4200D Calibrate...");
	fflush(stdout);
	float gyro_sum[ 3 ] = {0,0,0};
	S_Gyro gyro_read;
	int i;
	int div = 20;
	for(i=0 ; i < div ; i++)
	{
		L3G4200DRead(fd , &gyro_read);
		gyro_sum[ 0 ] = gyro_sum[ 0 ] + gyro_read.x;
		gyro_sum[ 1 ] = gyro_sum[ 1 ] + gyro_read.y;
		gyro_sum[ 2 ] = gyro_sum[ 2 ] + gyro_read.z;
	}
	gyro_calib.x = (float)(gyro_sum[ 0 ] / div);
	gyro_calib.y = (float)(gyro_sum[ 1 ] / div);
	gyro_calib.z = (float)(gyro_sum[ 2 ] / div);
	gyro_calibrate = 1;

	printf("\rL3G4200D Calibrate   \n");
}

//Check for new data
static unsigned char L3G4200DSetDRDY( int fd )
{
	return drv_i2cReadByte(fd, L3G4200D_STATUS_REG) & L3G4200D_ZYXDA;
}

void L3G4200DRead( int fd ,  S_Gyro * gyro)
{
	drv_i2cSetDevice(fd, L3G4200D_ADDRESS);
	if(L3G4200DSetDRDY( fd ) == L3G4200D_ZYXDA)
	{
		gyro->x = drv_i2cReadShort(fd, L3G4200D_OUT_X_L);
		gyro->y = drv_i2cReadShort(fd, L3G4200D_OUT_Y_L);
		gyro->z = drv_i2cReadShort(fd, L3G4200D_OUT_Z_L);
		if(gyro_calibrate == 1)
		{
			gyro->x = gyro->x - gyro_calib.x;
			gyro->y = gyro->y - gyro_calib.y;
			gyro->z = gyro->z - gyro_calib.z;

			gyro->x = gyro->x * SENSIVITY_L3G4200D_250DPS;
			gyro->y = gyro->y * SENSIVITY_L3G4200D_250DPS;
			gyro->z = gyro->z * SENSIVITY_L3G4200D_250DPS;
		}
	}
}
